#include "calibration/calibration.h"

struct ellipsoid_param mag_cali_res = {
	.scale = {0.0f},
	.offset = {0.0f}
};

struct ellipsoid_param acc_cali_res = {
	.scale = {0.0f},
	.offset = {0.0f}
};

void assign_cali_param(void)
{
	PARAM_ASSIGN(MAG_SCALE_X, PARAM_FLOAT, mag_cali_res.scale[0]);
	PARAM_ASSIGN(MAG_SCALE_Y, PARAM_FLOAT, mag_cali_res.scale[1]);
	PARAM_ASSIGN(MAG_SCALE_Z, PARAM_FLOAT, mag_cali_res.scale[2]);
	PARAM_ASSIGN(MAG_OFFSET_X, PARAM_FLOAT, mag_cali_res.offset[0]);
	PARAM_ASSIGN(MAG_OFFSET_Y, PARAM_FLOAT, mag_cali_res.offset[1]);
	PARAM_ASSIGN(MAG_OFFSET_Z, PARAM_FLOAT, mag_cali_res.offset[2]);

	PARAM_ASSIGN(ACC_SCALE_X, PARAM_FLOAT, acc_cali_res.scale[0]);
	PARAM_ASSIGN(ACC_SCALE_Y, PARAM_FLOAT, acc_cali_res.scale[1]);
	PARAM_ASSIGN(ACC_SCALE_Z, PARAM_FLOAT, acc_cali_res.scale[2]);
	PARAM_ASSIGN(ACC_OFFSET_X, PARAM_FLOAT, acc_cali_res.offset[0]);
	PARAM_ASSIGN(ACC_OFFSET_Y, PARAM_FLOAT, acc_cali_res.offset[1]);
	PARAM_ASSIGN(ACC_OFFSET_Z, PARAM_FLOAT, acc_cali_res.offset[2]);
}

uint8_t mag_calibrate_compensate(struct mag_raw_data *mag, uint8_t do_cali)
{
	static uint8_t save_result = 0;
	uint8_t cali_status = mag_check_calibration(mag->mag,
						    &mag_cali_res,
						    do_cali);
	if (CALI_DONE == cali_status) {
		if (save_result != 0) {
			param_save(MAG_SCALE_X);
			param_save(MAG_SCALE_Y);
			param_save(MAG_SCALE_Z);
			param_save(MAG_OFFSET_X);
			param_save(MAG_OFFSET_Y);
			param_save(MAG_OFFSET_Z);
		}
		mag_compensation(mag->mag, &mag_cali_res);
		return CALI_DONE;
	}
	save_result = 1;
	return cali_status;
}

uint8_t acc_calibrate_compensate(struct imu_raw_data *imu, uint8_t do_cali)
{
	static uint8_t save_result = 0;
	uint8_t cali_status = acc_calibration(imu->acc,
					      &acc_cali_res,
					      do_cali);
	if (CALI_DONE == cali_status) {
		if (save_result != 0) {
			param_save(ACC_SCALE_X);
			param_save(ACC_SCALE_Y);
			param_save(ACC_SCALE_Z);
			param_save(ACC_OFFSET_X);
			param_save(ACC_OFFSET_Y);
			param_save(ACC_OFFSET_Z);
		}
		acc_compensation(imu->acc, &acc_cali_res);
		return CALI_DONE;
	}
	save_result = 1;
	return cali_status;
}


